package piechowicz.szymon.balloonwars;

import com.google.android.maps.GeoPoint;

public class GeoMath
{
    
    // Mean radius = 6,371.0 km; Equatorial radius = 6,378.1 km
    private static final double R = 6375;
    
    // Converting deg <=> rad, cartesian <=> polar
    
    public static double deg2rad(double deg)
    {
      return deg * Math.PI / 180.0;
    }

    public static double rad2deg(double rad)
    {
      return rad / Math.PI * 180.0;
    }

    private static CartesianPoint polar2cartesian(GeoPoint point)
    {
        double lonRad = deg2rad((double)point.getLongitudeE6() / 1000000);
        double latRad = deg2rad((double)point.getLatitudeE6() / 1000000);
        return new CartesianPoint(
                Math.cos(lonRad) * Math.cos(latRad),
                Math.sin(lonRad) * Math.cos(latRad),
                Math.sin(latRad));
    }

    private static GeoPoint cartesian2polar(CartesianPoint point)
    {
        double lonRad = Math.atan2(point.y, point.x);
        double latRad = Math.asin(point.z);
        return new GeoPoint((int)(rad2deg(latRad) * 1000000),
                (int)(rad2deg(lonRad) * 1000000));
    }

    // Obtaining Great Circle points
    
    // bearing: south = -180, west = -90, north = 0, east = 90
    // distance: in km
    public static GeoPoint getGreatCirclePoint(GeoPoint start,
            double bearing, double distance)
    {
        CartesianPoint startCart = polar2cartesian(start);
        CartesianPoint p90 = getGreatCirclePoint90DegAway(start, bearing);
        double phiRad = distance / R;
        double cos = Math.cos(phiRad);
        double sin = Math.sin(phiRad);
        return cartesian2polar(new CartesianPoint(
                startCart.x * cos + p90.x * sin,
                startCart.y * cos + p90.y * sin,
                startCart.z * cos + p90.z * sin));
    }
    
    private static CartesianPoint getGreatCirclePoint90DegAway(GeoPoint start,
            double bearing)
    {
        GeoPoint pSouth = (start.getLatitudeE6() >= 0
                ? new GeoPoint(start.getLatitudeE6() - 90000000,
                        start.getLongitudeE6())
                : new GeoPoint(-start.getLatitudeE6() - 90000000,
                        start.getLongitudeE6() + 180000000));
        GeoPoint pWest = new GeoPoint(0, start.getLongitudeE6() - 90000000);
        CartesianPoint pSouthCart = polar2cartesian(pSouth);
        CartesianPoint pWestCart = polar2cartesian(pWest);
        double cos = Math.cos(deg2rad(bearing + 180));
        double sin = Math.sin(deg2rad(bearing + 180));
        return new CartesianPoint(
                pSouthCart.x * cos + pWestCart.x * sin,
                pSouthCart.y * cos + pWestCart.y * sin,
                pSouthCart.z * cos + pWestCart.z * sin);

    }
    
    // Classes
    
    private static class CartesianPoint
    {
        
        public final double x;
        public final double y;
        public final double z;
        
        public CartesianPoint(double x, double y, double z)
        {
            this.x = x;
            this.y = y;
            this.z = z;
        }
        
    }
    
}
